/*
 ------------------------------------------------------------------------------
 Copyright (C) 1996-2001 Id Software, Inc.

 This file is part of the Quake source code.

 The Quake source code is free software; you can redistribute it and/or
 modify it under the terms of the GNU General Public License as published by
 the Free Software Foundation; either version 2 of the License, or (at your
 option) any later version.

 The Quake source code is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
 more details.

 You should have received a copy of the GNU General Public License along with
 the Quake source code; if not, write to the Free Software Foundation, Inc.,
 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 ------------------------------------------------------------------------------
*/


//
// quaternion.cpp - quaternion math library
//


#include "../shared.h"


glqQuat		quat_identity(0.0f, 0.0f, 0.0f, 1.0f);


/*
 ==================
 glqQuat::ToAngles
 ==================
*/
glqAngles glqQuat::ToAngles (void) const {

	glqAngles	angles;
	float		xx, yy, zz;
	float		xy, xz, yz;
	float		wx, wy, wz;
	float		x2, y2, z2;
	float		s, c, theta;

	x2 = x + x;
	y2 = y + y;
	z2 = z + z;

	xx = x * x2;
	yy = y * y2;
	zz = z * z2;

	xy = x * y2;
	xz = x * z2;
	yz = y * z2;

	wx = w * x2;
	wy = w * y2;
	wz = w * z2;

	if (xz + wy > 1.0f)
		s = 1.0f;
	else if (xz + wy < -1.0f)
		s = -1.0f;
	else
		s = xz + wy;

	theta = -glqMath::ASin(xz + wy);
	c = glqMath::Cos(theta);

	if (c > 8192.0f * M_EPSILON){
		angles.pitch = RAD2DEG(theta);
		angles.yaw = RAD2DEG(glqMath::ATan(xy - wz, 1.0f - (yy + zz)));
		angles.roll = RAD2DEG(glqMath::ATan(yz - wx, 1.0f - (xx + yy)));
	}
	else {
		angles.pitch = RAD2DEG(theta);
		angles.yaw = RAD2DEG(-glqMath::ATan(xy + wz, 1.0f - (xx + zz)));
		angles.roll = 0.0f;
	}

	return angles;
}

/*
 ==================
 glqQuat::ToMat3
 ==================
*/
glqMat3 glqQuat::ToMat3 (void) const {

	glqMat3	mat;
	float	xx, yy, zz;
	float	xy, xz, yz;
	float	wx, wy, wz;
	float	x2, y2, z2;

	x2 = x + x;
	y2 = y + y;
	z2 = z + z;

	xx = x * x2;
	yy = y * y2;
	zz = z * z2;

	xy = x * y2;
	xz = x * z2;
	yz = y * z2;

	wx = w * x2;
	wy = w * y2;
	wz = w * z2;

	mat[0][0] = 1.0f - (yy + zz);
	mat[0][1] = xy - wz;
	mat[0][2] = xz + wy;
	mat[1][0] = xy + wz;
	mat[1][1] = 1.0f - (xx + zz);
	mat[1][2] = yz - wx;
	mat[2][0] = xz - wy;
	mat[2][1] = yz + wx;
	mat[2][2] = 1.0f - (xx + yy);

	return mat;
}

/*
 ==================
 glqQuat::ToMat4
 ==================
*/
glqMat4 glqQuat::ToMat4 (void) const {

	glqMat4	mat;
	float	xx, yy, zz;
	float	xy, xz, yz;
	float	wx, wy, wz;
	float	x2, y2, z2;

	x2 = x + x;
	y2 = y + y;
	z2 = z + z;

	xx = x * x2;
	yy = y * y2;
	zz = z * z2;

	xy = x * y2;
	xz = x * z2;
	yz = y * z2;

	wx = w * x2;
	wy = w * y2;
	wz = w * z2;

	mat[0][0] = 1.0f - (yy + zz);
	mat[0][1] = xy + wz;
	mat[0][2] = xz - wy;
	mat[0][3] = 0.0f;
	mat[1][0] = xy - wz;
	mat[1][1] = 1.0f - (xx + zz);
	mat[1][2] = yz + wx;
	mat[1][3] = 0.0f;
	mat[2][0] = xz + wy;
	mat[2][1] = yz - wx;
	mat[2][2] = 1.0f - (xx + yy);
	mat[2][3] = 0.0f;
	mat[3][0] = 0.0f;
	mat[3][1] = 0.0f;
	mat[3][2] = 0.0f;
	mat[3][3] = 1.0f;

	return mat;
}

/*
 ==================
 glqQuat::ToString

 This is just a convenience function for printing quaternions
 ==================
*/
const char *glqQuat::ToString (int precision) const {

	return Str_FromFloatArray(ToFloatPtr(), 4, precision);
}